#include <Wire.h>
#include "BluetoothSerial.h"
#include <Adafruit_PWMServoDriver.h>
// 初始化16路驱动板
BluetoothSerial SerialBT;
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x40);    //Create an object of board 1
Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x41);    //Create an object of board 2 (A0 Address Jumper)



#define servoFrequency 60  // Servo update frequency, analog servos typically run at ~50 Hz

void setup() {
    //创建一个蓝牙并命名
    SerialBT.begin("HUMAN_BODY");
    //设置usb端口频率
    Serial.begin(115200);
    setPwm();
    // 打印信息
    toPrint("body");
}


void loop() {
  // 如果蓝牙串口有信号输入
    if (SerialBT.available()) {
      // 读取 f
        char f = SerialBT.read();
        // 读取数字
        char num = SerialBT.read();
        // 读取小数
        float value = SerialBT.parseFloat();
        char n = SerialBT.read();
        // 读取结束
        String fun = String(f);
        // 拼接字符串
        String printable = fun + String(num) + String(value) + String(n);
//        toPrint(printable);
// 如果f是个z,代表打印信息
        if (fun == "z") {
            getInfo("blue");
        } else {
          // 设置舵机
            setServo(fun, num, value);
        }
    }
    if (Serial.available()) {
        char f = Serial.read();
        char num = Serial.read();
        float value = Serial.parseFloat();
        char n = Serial.read();
        String fun = String(f);
        String printable = fun + String(num) + String(value) + String(n);
//        toPrint(printable);
        if (fun == "z") {
            getInfo("usb");
        } else {
            setServo(fun, num, value);
        }
    }
}
void toPrint(String msg) {
    Serial.println(msg);
    SerialBT.print(msg);
}
void setPwm() {
    pwm.begin();
    pwm.setPWMFreq(servoFrequency);
    pwm1.begin();
    pwm1.setPWMFreq(servoFrequency);
    pwm2.begin();
    pwm2.setPWMFreq(servoFrequency);
}
void setServo(String fun, char n, float rot) {
  // 通过n信息获取第几个舵机
    int num = getInputValue(n);
    // fun代表第几个版
    if (fun == "a") {
        pwm.setPWM(0, num, rot);
    } else if (fun == "b") {
        pwm1.setPWM(0, num, rot);
    } else if (fun == "c") {
        pwm2.setPWM(0, num, rot);
    } else {
        toPrint("未知pwm值");
    }
};

void getInfo(String source) {
    String info = "{source:" + source + ",type;body}";
    toPrint(info);
}

int getInputValue(char input) {
    int value;
    switch (input) {
        case 'a':
            value = 1;
            break;
        case 'b':
            value = 2;
            break;
        case 'c':
            value = 3;
            break;
        case 'd':
            value = 4;
            break;
        case 'e':
            value = 5;
            break;
        case 'f':
            value = 6;
            break;
        case 'g':
            value = 7;
            break;
        case 'h':
            value = 8;
            break;
        case 'i':
            value = 9;
            break;
        case 'j':
            value = 10;
            break;
        case 'k':
            value = 11;
            break;
        case 'l':
            value = 12;
            break;
        case 'm':
            value = 13;
            break;
        case 'n':
            value = 14;
            break;
        case 'o':
            value = 15;
            break;
        case 'p':
            value = 16;
            break;
        case 'q':
            value = 17;
            break;
        case 'r':
            value = 18;
            break;
        case 's':
            value = 19;
            break;
        case 't':
            value = 20;
            break;
        case 'u':
            value = 21;
            break;
        case 'v':
            value = 22;
            break;
        case 'w':
            value = 23;
            break;
        case 'x':
            value = 24;
            break;
        case 'y':
            value = 25;
            break;
        case 'z':
            value = 26;
            break;
        default:
            value = -1; // 返回一个无效输入的值
            break;
    }
    return value;
}